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Graph-based  Observability  Analysis  of  Bearing-only 
Cooperative  Localization 

Rajnikant  Sharma,  Randy  Beard,  Clark  Taylor,  and  Stephen  Quebe 


Abstract — In  this  paper  we  investigate  the  nonlinear  observability 
properties  of  bearing-only  cooperative  localization.  We  establish  a  link 
between  observability  and  a  graph  representing  measurements  and 
communication  between  the  robots.  It  is  shown  that  graph  theoretic 
properties  like  the  connectivity  and  the  existence  of  a  path  between  two 
nodes  can  be  used  to  explain  the  observability  of  the  system.  We  obtain 
the  maximum  rank  of  the  observability  matrix  without  global  information 
and  derive  conditions  under  which  the  maximum  rank  can  be  achieved. 
Furthermore,  we  show  that  for  complete  observability,  all  of  the  nodes 
in  the  graph  must  have  a  path  to  at  least  two  different  landmarks  of 
known  location. 


I.  Introduction 

In  cooperative  localization  a  group  of  robots  exchange  relative 
position  measurements  from  their  exteroceptive  sensors  (e.g.,  camera, 
laser,  etc.)  and  their  motion  information  (velocity  and  turn  rate) 
from  interoceptive  sensors  (e.g..  IMU,  encoders,  etc.)  to  collectively 
estimate  their  states.  Cooperative  localization  has  been  an  active  area 
of  research  (e.g.,  [  1  ] — [8])  because  it  provides  several  potential  ad¬ 
vantages,  including  increased  localization  accuracy,  sensor  coverage, 
robustness,  efficiency,  and  flexibility. 

Recently,  estimation  algorithms  such  as  the  Extended  Kalman 
Filter  (EKF)  [9],  Minimum  Mean  Square  Estimator  (MMSE)  [2], 
Maximum  Likelihood  Estimation  (MLE)  [10],  Particle  Filter  [11], 
and  Maximum  A  Posteriori  (MAP)  [12],  have  been  used  to  solve 
the  cooperative  localization  problem.  These  algorithms  can  be  used 
either  in  a  centralized  [5]  or  decentralized  manner  [2],  [9],  [12],  For 
the  localization  errors  to  be  bounded,  it  is  required  that  the  system 
be  observable,  independent  of  the  estimation  technique  being  used. 

Several  authors  have  carried  out  observability  analysis  of  the  coop¬ 
erative  localization  problem.  Initial  results  regarding  the  observability 
of  cooperative  localization  were  reported  by  Roumeliotis  and  Bekey 
[9],  They  used  linear  observability  analysis  to  show  that  the  states  of 
the  robots  performing  cooperative  localization  are  unobservable,  but 
can  be  made  observable  by  providing  global  positioning  information 
to  one  of  the  robots.  In  [9]  it  was  assumed  that  the  absolute  vehicle 
heading  is  measured  directly  and  does  not  need  to  be  estimated. 
Furthermore,  linear  approximation  of  a  nonlinear  system  can  provide 
different  structural  properties  regarding  the  observability  [13],  [14]. 
Martinelli  et  al.  [15]  investigates  the  nonlinear  observability  of  coop¬ 
erative  localization  for  two  robots  without  heading  measurements. 
They  compared  the  observability  properties  of  range  and  bearing 
measurements  and  showed  that  with  either  type  of  measurement, 
the  maximum  rank  of  the  observability  matrix  is  three,  i.e.,  not 
fully  observable.  The  analysis  in  [15]  shows  that  relative  bearing 
is  the  best  observation  between  the  robots.  The  part  of  the  system 
which  is  observable  is  in  general  larger  than  for  the  other  relative 
observations  (relative  distance  and  relative  orientation).  Accordingly, 
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[15]  uses  polar  coordinates  for  the  observability  analysis.  Although 
polar  coordinates  simplify  the  analysis  for  two  robots,  we  use  a 
global  coordinate  system  because  it  is  more  appropriate  for  graph 
level  ( n  >  2)  observability  analysis. 

In  this  paper,  we  extend  the  observability  analysis  presented  in  [15] 
from  2  to  n  robots,  with  bearing-only  measurements.  The  extension 
for  n  >  2  is  not  obvious  because  of  the  dynamically  changing  set 
of  n(n  —  l)/2  different  relative  bearing  measurements  leading  to 
2«,(n— 1)/2  p0SSjye  configurations.  Furthermore,  since  the  robot  states 
in  [15]  are  not  observable  with  respect  to  a  global  reference  frame, 
and  since  it  has  been  shown  that  two  landmarks  are  needed  for  the 
observability  of  a  single  vehicle  [  16]— [  18],  in  this  paper  we  derive 
the  number  of  landmarks  needed  for  full  observability  of  a  group  of 
n  robots  performing  cooperative  localization.  In  contrast  to  [9],  we 
also  assume  that  the  heading  of  each  robot  is  not  directly  measured 
but  must  be  estimated. 

To  represent  a  group  of  robots,  we  will  use  the  Relative  Position 
Measurement  Graph  (RPMG)  introduced  in  [19].  The  nodes  of  an 
RPMG  represent  vehicle  states  and  the  edges  represent  bearing 
measurements  between  nodes.  We  establish  a  relationship  between 
the  graph  properties  of  the  RPMG  and  the  rank  of  the  system 
observability  matrix.  We  prove  that  for  a  connected  RPMG,  the 
observability  matrix  for  a  team  of  n  robots,  which  has  size  3 n  x  3 n 
will  have  rank  3  (n  —  1).  We  also  derive  conditions  under  which 
landmarks  observed  by  a  subset  of  robots  enable  the  system  to 
become  fully  observable. 

The  paper  is  organized  as  follows.  In  Section  II  we  describe 
bearing-only  cooperative  localization  and  formulate  the  problem.  In 
Section  III  we  perform  the  nonlinear  observability  analysis.  In  Section 
IV  we  give  our  conclusions. 

II.  Bearing-only  Cooperative  Localization 

Consider  n  robots  moving  in  a  horizontal  plane  performing  coop¬ 
erative  localization.  We  can  write  the  equations  of  motion  for  the  ith 
robot  as, 

(Vi  cos  ipi  \ 

Vi  sin  ipi  1  ,  (1) 

where  X P  =  [xi  yi  ipi]  G  R3  is  the  robot  state,  including  robot 
location  ( Xi ,  yi)  and  robot  heading  ipi,  and  m  =  [V),ui;]T  is  the 
control  input  vector.  We  assume  that  onboard  introspective  sensors 
(e.g.,  encoders)  measure  the  linear  speed  Vi  and  angular  speed  u>i 
of  the  robot.  Without  loss  of  generality,  we  assume  that  robots 
cannot  move  backward  (Vi  >0,  i  =  1  •  •  •  n).  Each  vehicle  has  an 
exteroceptive  sensor  to  measure  relative  bearing  to  other  vehicles  and 
known  landmarks  that  are  in  the  field-of-view  of  the  sensor.  Relative 
bearing  from  the  ith  robot  to  the  jth  robot  or  landmark  can  be  written 
as, 

rnj  =  tan-1  (— — —  ^  -  ipi.  (2) 

\Xj  -  Xi) 

For  cooperative  localization,  each  robot  exchanges  its  local  sensor 
measurements  (velocity,  turn  rate,  and  bearing  to  landmarks  and  other 
robots)  with  their  neighbors.  Let  iV,M  be  the  set  of  neighbors  for 
which  robot  i  can  obtain  bearing  measurements,  and  let  Nf  be  the 
set  of  neighbors  with  which  robots  i  can  communicate.  In  this  paper, 
we  assume  that  Nf1  =  Nf  and  we  will  therefore  denote  the  set  of 
neighbors  as  Ni.  To  represent  the  connection  topology  of  the  robots 
we  use  a  relative  position  measurement  graph  (RPMG)  [19]  which 
is  defined  as  follows. 

Definition  1:  An  RPMG  for  n  robots  performing  cooperative 
localization  with  l  different  known  landmarks  is  a  directed  graph 
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Fig.  1.  Relative  position  measurement  graph  (RPMG).  The  nodes  of  an 
RPMG  represent  vehicle  states  and  the  edges  represent  bearing  measurements 
between  nodes. 

Gln  =  { Vn,i,Sn,i },  where  Vn,i  =  {1,  •  •  •  ,  n,  n  +  1,  •  •  •  ,  n  +  1} 
is  the  node  set  consisting  of  n  robot  nodes  and  l  landmark  nodes 
and  £n,i(t)  C  {Vn,o  x  Vn,i}  =  {ytj},  *  £  {1,  ,n},  j  G 

{ 1 ,  •  •  •  ,n,n+  1,  •  •  •  ,n  +  1}  is  the  edge  set  representing  the  avail¬ 
ability  of  a  relative  bearing  measurement.  We  use  m  to  denote  the 
number  of  edges  in  the  RPMG.  An  example  RPMG  (G3  with  m  =  7) 
is  shown  in  Fig.  1. 

Additionally,  without  loss  of  generality  we  assume  that  robots 
maintain  a  safe  distance  from  each  other  and  from  landmarks,  i.e., 
Rij  >  0,  Vi,j  =  1  ,  •••n  and  landmarks  Rik  >  0,  Vi  = 
1,  ■  •  •  n;  k  =  1,  •  •  •  ,  l. 

A.  Lie  Derivatives  and  Nonlinear  Observability 

To  determine  the  observability  of  the  entire  system  represented  by 
the  RPMG  we  use  the  nonlinear  observability  rank  criteria  developed 
by  Flermann  and  Rrener  [20]  which  is  summarized  in  the  next 
paragraph. 

Consider  a  system  model  with  the  following  form 

X  =  g(X,u)»  ,gJ(Xn,un)\T  ,,, 

y  =  h(X,Z)  =  [hT(X,Z)---hUx,Z)]T 

where  X  =  [Xi  Xj  ■  ■  ■  X^Y  G  R3n  is  the  state  of  the  system, 
Z  =  [Zj  Zj  ■  ■  ■  Zj  ]T  G  R21  is  the  position  vector  of  all  landmarks, 
Zi  =  [xi  t/i]T  is  the  position  vector  of  ith  landmark,  hi  :  R3n  x 
R2i  i->  R  is  the  measurement  model  of  the  ith  sensor,  u  G  A  C  R2ti 
is  the  control  input  vector,  and  g  :  R3n  x  A  i-»  R3".  We  consider 
the  special  case  where  the  process  function  g  can  be  separated  into  a 
summation  of  independent  functions,  each  one  excited  by  a  different 
component  of  the  control  input  vector,  i.e., 

X  =  g(X,  u)  =  fVlV 1  +  /ujjUtl  +  '  ■  '  +  fvnVn  +  fuinOJn  (4) 

The  zeroth-order  Lie  derivative  of  any  (scalar)  function  is  the  function 
itself,  i.e.,  L°hk(X,  Z)  =  hk{X,Z).  The  first-order  Lie  derivative 
of  function  hk(X ,  Z)  with  respect  to  fVi  is  defined  as 

L)v.h  =  XL°h-  fVi  (5) 

V  represents  the  gradient  operator,  and  ■  denotes  the  vector  inner 
product.  Considering  that  hk(X,  Z)  is  a  scalar  function  itself, 
the  second-order  Lie  derivative  of  hk  ( X ,  Z)  with  respect  to  is 

Lfv  fv.  h  =  XLXfv.  h  •  fVi.  (6) 

Fligher  order  Lie  derivatives  are  computed  similarly.  Additionally,  it  is 
possible  to  define  mixed  Lie  derivatives,  i.e.,  with  respect  to  different 
functions  of  the  process  model.  For  example,  the  second-order  Lie 


derivative  of  hk  with  respect  to  fVj,  given  its  first  derivative  with 
respect  to  fVi,  is 

Lliujh  =  VL1uX-fvy  (7) 

Based  on  the  preceding  expressions  for  the  Lie  derivatives  the 
observability  matrix  is  defined  as  the  matrix  with  rows 

O  =  hk(X,Z)}  (8) 

where  i,j  =  1,  •  •  •  ,n;fc  =  1,  •  ■  •  ,rtv,p  G  N.  The  important  role 
of  this  matrix  in  the  observability  analysis  of  a  nonlinear  system  is 
demonstrated  by  Theorem  1. 

Theorem  1:  A  system  is  locally  weakly  observable  if  its  observ¬ 
ability  matrix  whose  rows  are  given  in  (8)  has  full  rank,  e.g.,  in  our 
case  rank(0)  =  3 n. 

Additionally,  we  assume  that  the  robot  sensors  have  limited  sensor 
range  p3e„3or  and  limited  field  of  view.  Therefore,  agents  can  only 
measure  the  bearing  of  those  robots  and  landmarks  that  are  located 
within  the  footprint  of  the  sensor.  Therefore,  the  graph  Gln  will  likely 
have  a  time  varying  topology. 

III.  Graph-based  Observability  Analysis 

In  this  section,  we  obtain  the  conditions  for  the  observability  of 
the  graph  G„.  We  derive  explicit  conditions  that  establish  the  rank 
of  the  observability  matrix  of  the  graph  G°  without  landmarks,  and 
the  number  of  landmarks  needed  for  the  full  rank  of  the  observability 
matrix  of  the  graph  Gln. 


A.  Rows  in  the  Observability  matrix  due  to  an  Edge 

In  a  graph  Gln  there  are  two  types  of  edges:  an  edge  between  two 
robots,  and  an  edge  between  a  robot  and  a  landmark.  We  derive  the 
maximum  number  of  linearly  independent  rows  in  the  observability 
sub-matrix  of  an  edge  and  the  conditions  for  the  maximum  rank  of  the 
observability  sub-matrix  of  an  edge.  The  linearly  independent  rows 
of  the  observability  sub-matrix  of  an  edge  serve  as  building  block 
for  the  observability  conditions  for  the  graph  Gln. 

1 )  Edge  between  two  robots:  First  we  derive  the  linearly  indepen¬ 
dent  rows  in  the  observability  matrix  for  an  edge  r/ij  between  two 
robots  and  derive  the  conditions  under  which  maximum  number  of 
linearly  independent  rows  can  be  obtained. 

We  first  find  the  Lie  derivatives  of  i pj.  We  rearrange  the  nonlinear 
kinematic  equations  in  the  following  convenient  form  for  computing 
Lie  derivatives: 


where  fVi  =  [«/>;  st/);  0  0  0  0]T,  fuli  =  [0  0  1  0  0  0]T,  fVj  = 
[0  0  0  cipj  sipj  0]T,  fuij  =[0  0  0  0  0  1]T,  ctpi  =  costpi,  and 
sipi  =  sin  tpi.  We  hereafter  compute  the  necessary  Lie  derivatives  of 
r/ij  and  their  gradients. 

Zeroth-order  Lie  derivative 


—  fvi  Vi  +  fUiuJi  +  fVj  Vj  +  fuijtOj  (9) 


L°h  =  r/ij 

and  gradient  scaled  by  J?2^  is  given  by 

VL°h=  [  —A y-ij  Axij  —Rij  Ayij  —A Xij  0  ] 

where,  A  Xij  =  Xi  —  Xj,Ayij  =  yt  —  yj,  and  R^j  =  (A  Xij)2  + 

(A  y,j)2. 

Remark  1:  The  scaling  by  Rij  is  an  elementary  row  operation, 
therefore,  it  does  not  change  the  space  spanned  by  the  rows  of  the 
observability  matrix.  Also,  it  simplifies  the  computation  of  the  higher 
order  Lie  derivatives. 
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First-order  Lie  derivatives 

L)vi  h  =  VL°ft  •  fVi  =  A XijStpi  —  A UijCi/ti 
L)Vj  h  =  VL°ft  •  fVj  =  -(A XijStpj  -  Ay  ij  ctpj) 

L/w.  ft  =  VL°ft  •  fUi  =  -Rfj 
L)  ft  =  VL°ft  •  =  0 

with  gradients  given  by 

VL}  ft  =  [  st/>i  —  cijji  Jf  —stpi  ctpi  0  ] 

cVb  0  stpj  -ctpj  -Jf  } 

VL}o  .ft  =  2  [  —A Xij  — Aj/y  0  Aa:y  At/y  0  ] 
where  =  A XijCtpi  +  Ay  tj  stpi  and  jf  =  Aa HjCtpj  +  Ay  ij  stpj. 
Second-order  Lie  derivatives 

L2fv.fv.  ft  =  VL/v.  ft  •  /„4  =  stpiCtpi  —  stpiCipi  =  0 
.  f  ft  =  VL}„ .  ft  •  fVj  =  stpjCtpj  —  stpjctpj  =  0 
L2fv.fv.  ft  =  VI/„.  ft  •  fVj  =  —stpictpj  +  stpjctpi 
L%tf»ih  =  VL)vh-fUi  =J+ 

L2f„.fvi  =  VL^.  ft  •  /„4  =  -2J+ 

=VLlih-fVj  =  2J+ 
with  gradients  given  by 

VL)Vifvh=  [  0  0  —  0  0  Jj,  ] 

V^/„ ,ful.h=  [  ctpi  stpi  J~  ctpi  stpi  0  ] 

VLf  r  ft  =  [  — —stpj  0  stpj  — ctpj  —Jf 

J  Vj  JUJj  1  J  J 

where  J,/,  =  ctpictpj  +  stpi  stpj,  Jf  =  Ay  ij  ctpi  —  Ax  ij  stpi,  and 
J-  =  Ay  ^  ctpj  -  A  Xij  stpj. 

Remark  2:  Gradients  of  ^  and  L2^  fv  are  not  included 
because  they  are  linearly  dependent  on  VL^  ^  ft  and  VLy  ^  ft 
respectively. 

Third-order  Lie  derivatives 

L3fv.fv  fWih  =  ^L2fv.fv.  ft  •  /u>4  =  {ctpictpj  +  stpistpj) 
L)Vifvjf„j  h  =  A,-  h’  f"j=  {ctPiCtPj  +  sV’isVb') 

L3fVif^fVih  =  VL2fvifwih-fVi  =  1 

L/y  f*j  fvj  h  =  /<y  h  '  /«J  =  1 

L3fVifUifUi  h  ft  •  /W4  =  -(A  XijStpi  -  AytjCtpi) 

'■l,  f. h  =  VL}v.fw,  ft  •  =  A  XijStPj  -  Ay  ij  ctpj 

with  gradients  given  by 

v^/V^  =  -“vz4i/t,.ft 

V ■E'/v4  /«4  /W<  h  =  -  V £/„4  h 

VLy  f  f  ft  —  — V  L\  ft. 

where  a  =  (stpictpj  —  ctpiStpj),  and  pi  =  ctpictpj  +  stpistpj. 

Clearly,  third  and  higher  order  Lie  derivatives  are  linearly  depen¬ 
dent  on  the  gradients  of  second  and  lower  order  Lie  derivatives. 
Therefore,  with  all  the  non-zero  inputs  the  observability  matrix  of 
an  edge  between  two  robots  can  be  written  using  gradients  of  Lie 


derivatives  up  to  second-order  as 
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0 

2A  Xij 

2A  yij 

0 

0 

0 

.  jy. 

0 

0 

J-lf} 

Ctpi 

Stpi 

Jr 

-ctpi 

-Stpi 

0 

o 

-e- 

-stpj 

0 

ctpj 

stpj 

-J7 

GO) 

Our  objective  is  to  find  the  number  of  linearly  independent  rows 
in  Oij .  Therefore,  we  transform  Oij  into  reduced  row  echelon  form 
(RREF).  RREF  is  the  simplest  possible  form  of  a  matrix,  which 
directly  provides  the  number  of  linearly  independent  rows  in  the 
matrix.  Since  RREF  is  the  backbone  of  the  analysis  presented  in 
this  paper  we  state  the  next  lemma,  which  explains  the  properties  of 
a  RREF  matrix. 

Lemma  1  {  [21]):  A  matrix  A  £  Rmxn,  by  means  of  a  finite 
sequence  of  elementary  row  operations,  can  be  transformed  to  a  row 
reduced  echelon  form  U  £  Rmxn  such  that 

EA  =  U  (11) 

where  E  £  Rmxm  is  the  elementary  operation  matrix.  If  the  rank  of 
A  is  r  then 


1) 

U  =  \  „  Ir  n  B  }  (12) 

"(m-r)xr  ”(m  —  r)y.(n  —  r') 

where  Ir  is  the  Identity  matrix  of  size  r  and  B  £  RT’x(”-r\ 

2)  the  first  r  rows  of  matrix  U  are  linearly  independent, 

3)  the  non  zero  rows  of  the  matrix  U  spans  the  same  row  space 
spanned  by  A, 

4)  if  A  is  an  invertible  matrix  (  r  =  m  =  n)  then  U  is  the  Identity 
matrix. 

The  next  lemma  provides  conditions  for  the  maximum  rank  of  the 
observability  matrix  of  an  edge  between  two  robots. 

Lemma  2:  The  rank  of  Oij  given  by  (10)  (edge  between  two 
robots)  is  three  if 

1)  Vi  >  0, 

2)  Vj  >  0, 

3)  the  ith  robot,  which  is  measuring  the  bearing,  does  not  move 
along  the  line  joining  the  two  robots, 

4)  the  jth  robot  does  not  move  perpendicular  to  the  line  joining 
the  two  robots. 

Proof:  To  prove  the  lemma,  first  we  write  Jf  and  jJ  as 

Jf  =vjvi  =  A yij  cos  tpi  —  Axij  sin  tpi  (13) 

Jj+  =  vj  Vj  =  A  Xij  cost pj  +  A  y^  sin  tpj  (14) 

where  ui  =  [Aa Hj  A yij]T  is  a  vector  along  the  line  between  the 
two  robots,  Vj  =  [cos  tpj  sini/;j]T  is  the  heading  vector  of  the  jth 
robot,  and  v,  =  [—  sin  tpi  cosi/>j]T  is  a  vector  perpendicular  to  the 
heading  vector  of  the  ith  robot. 

Front  (13)  and  (14),  we  can  verify  that  if  the  ith  robot,  which  is 
measuring  the  bearing,  does  not  move  along  the  line  joining  the  two 
robots  then  J~  ^  0,  and  if  the  jth  robot  does  not  move  perpendicular 

to  the  line  joining  the  two  robots  then  jf  0.  We  then  use  the 
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elementary  operation  matrix 


A  Vij 

+  ■<. 

1  -s 

+  ■* 

1  -s 

4 

s4>j 

si/jjRL 

x  i  j 

1 

*■*■  + 

1 

.  '-H 

~4~ 

Co 

y 

1 

1 

1 

*•  1 
Vh 
J’  + 

4 

2jf 

0 

J~ 

Ji 

1  S(2lpi-23l2j) 

2  Ji  Jt 

Jt 

1 

0 

Ji 

JT 

jf 

1 

1 

4 

which  transforms  Oij  as 


0  0  0  0 
0  0  0  0 
0  0  0  0 
10  0  0 
0  10  0 
0  0  10 
0  0  0  1 


EijOij  —  Uij 


I3  Oij 
04x3  04x3 


(15) 


where 


()''  = 


-1  0  A  y-ij 

0  —1  A  Xij 

0  0-1 


(16) 


From  Lemma  1  we  can  say  that  RREF  matrix  Uij  has  three  linearly 
independent  rows  and  these  rows  span  the  same  observability  space 
spanned  by  rows  of  Oij ,  therefore,  maximum  rank  of  Oij  is  three.  It 
should  be  noted  that  the  top  three  non-zero  rows  in  Uij  corresponds 
to  L°h,  L^v  h,  and  L^v  h,  therefore,  conditions  of  the  Lemma  2  are 
the  sufficient  conditions  for  rank(Oij)  =  3.  ■ 


2)  Edge  between  a  robot  and  a  landmark:  In  this  section,  we 
derive  the  linearly  independent  rows  in  the  observability  matrix  for 
an  edge  ryk  between  a  robot  and  a  landmark  and  derive  the  conditions 
under  which  maximum  number  of  linearly  independent  rows  can 
be  obtained.  We  rearrange  the  nonlinear  kinematic  equations  in  the 
following  convenient  form  for  computing  Lie  derivatives: 


Xi  —  fVtVi  +  fun<jJi  (17) 

where  fVi  =  [cipi  sipi  0]T  and  fUi  =  [0  0  1]T.  We  hereafter  compute 
the  necessary  Lie  derivatives  of  r/ik  and  their  gradients. 
Zeroth-order  Lie  derivative 

L°h  =  i)ik 

and  its  gradient,  scaled  by  R2tk  is  given  by 

VL°/i  =  [  -A yik  A Xik  -Rik  ] 

where,  A  Xik  =  Xi  —  Xk,  Ayik  =  yt  -  yk,  and  R?ik  =  (A  Xik)2  + 
(Aj/ifc)  . 

First-order  Lie  derivatives 

L)v.  =  Ax-ikStpi  -  AytkCipi 

LfUi  =  —Rik 

with  gradient  given  by 

VL}u.  =  [  sipi  —cipi  Ax-ikCipi  +  Ay ik  sipi  ] 

VL/U.  =  2  [  -A Xik  -At Hk  0  ]  . 


Second-order  Lie  derivatives 

L2fv.fv.  h  =  sipiCipi  —  sipiCipi  =  0 
£/„./  h  =  AxikCipi  +  AyikSipi 

Llifvih  =  -2LliUih 


with  gradients  given  by 

=  [  cV’i  sipi  AytkCipi  -  AxikSipi  ]. 

Remark  3:  Gradient  of  L f  ,fv.h  is  not  included  because  it  is 
linearly  dependent  on  V L2v  f^  h. 

Third-order  Lie  derivatives 


-  1 

L3fVifUifUi  =  -( Ax^sipi  -  Ayikcipi)  =  -L}v. . 


Clearly,  the  gradients  of  third  and  higher  order  Lie  derivatives  are 
linearly  dependent  on  the  rows  of  the  observability  matrix  corre¬ 
sponding  to  second  and  lower  order  Lie  derivatives.  Therefore,  we 
can  write  the  rows  of  the  observability  matrix  corresponding  to  an 
edge  between  a  robot  and  a  landmark,  using  the  gradients  of  Lie 
derivatives  up  to  second  order,  as 


Oik  = 


A  yik  A  Xik  Rik 

Sipi  - Cipi  J+ 

-2A  -2A  yik  0 

Cipi  Sipi  J~ 


(18) 


where  J+  =  AxikCipi  +  A  yik  sipi  and  =  A  yikCipi  —  Ax^sipi. 

Lemma  3:  The  rank  Oik  given  by  (18)(edge  between  a  robot  and 
a  landmark)  is  two  if 


1)  Vi  >  0, 

2)  the  robot  does  not  move  along  the  line  joining  the  robot  and 
the  landmark. 


Proof:  If  the  robot  does  not  move  along  the  line  joining  the 
robot  and  the  landmark  then  J~  0  and  the  elementary  operation 
matrix 


Rik  — 


~c'4’i 

J- 

J- 

—  SiPi 

~AVil 

J- 

J-0 

-2J+ 

-2  Ril 

J- 

JT 

1 

J+ 

J- 

J- 

transforms  Oik  as 


EikOik  —  Uik  — 


Oik 

02x3 


where 


Oik  — 


1  0  A  yik 

0  1  —A  Xik 


(19) 


(20) 


It  should  be  noted  that  the  top  two  non-zero  rows  in  the  observability 
matrix  are  linearly  independent  (from  Lemma  1)  and  they  correspond 
to  L°h  and  .  Therefore,  V»  >  0  and  .J~  0  are  the  sufficient 

conditions  for  the  rank  of  the  observability  matrix  being  two.  ■ 
Definition  2:  An  RPMG  Gln  (Definition  1)  is  called  a  proper 
RPMG  if  all  the  edges  between  robot  nodes  satisfy  the  conditions 
of  Lemma  2  and  all  the  edges  between  robots  and  landmarks  satisfy 
Lemma  3. 

In  a  proper  RPMG  each  edge  ryj  between  two  robots  contribute 
three  linearly  independent  rows  to  the  observability  matrix  of  a  proper 
RPMG  and  each  edge  rjik  between  a  robot  and  a  landmark  contributes 
two  linearly  independent  rows  to  the  observability  matrix  of  a  proper 
RPMG.  Using  three  linearly  independent  rows  of  Uij  in  (15)  and 
two  linearly  independent  rows  of  Uik  in  (19),  we  can  write  the 
observability  matrix  of  a  proper  RPMG  Gln  as 


O  =  {  ^  | ,  i,j  =  l,-"  ,n;k  =  l,--  -  ,1  (21) 

where  Oij  =  [03x3(*-i)  I3  03x(3y_ij_3i)  Oij  03x3(n_j)]  and 
Oik  =  [C>2x3(i-1))  Oik  02x3(n-i)]- 
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▲  2 

/ 


lA - A3 

(a) 


A  A  \ 

lA  A3  iA - A3  iA - A3 

(b)  (C)  (d) 


Fig.  2.  The  observability  conditions  between  these  four  possible  conhgura- 
tions  of  a  connected,  3-node  RPMG  are  identical. 


Remark  4:  The  observability  matrix  O  in  (21)  is  not  the  original 
observability  matrix  of  the  graph  Gln.  Since  the  rows  of  (21)  consist  of 
the  linearly  independent  rows  after  elementary  row  operations,  from 
Lemma  1  we  know  that  the  rows  of  the  observability  matrix  in  (21) 
span  the  same  observable  space  spanned  by  the  original  observability 
matrix. 


(d)  (e)  (f) 


Fig.  3.  An  example  of  converting  an  arbitrary  connected  RPMG  to  a  2-level 
tree. 


B.  Observability  Analysis  Without  Landmarks 


In  this  section  we  derive  the  conditions  for  achieving  the  maximum 
rank  of  the  observability  matrix  for  the  graph  G°  without  landmarks. 
We  first  discuss  the  observability  properties  for  a  3-node  graph  G3. 

Lemma  4:  If  a  three  node  proper  RPMG  G3  is  connected,  then 
the  rank  of  the  observability  matrix  is  six. 

Proof:  There  are  four  possible  configurations  of  a  connected 
graph  G°,  shown  as  sub-figures  (a)  through  (d)  in  Fig.  2.  We  can 
write  the  transformed  observability  matrix  for  these  configurations 
using  (21)  as 


Oa  = 
oc  = 


012 

0 

0 

013 

Ob  = 

Is 

0 

012 

I3 

0 

023 

012 

Is 

0 

0 

023 

013 

Od  = 

Is 

0 

0 

Is 

013 

023 

We  perform  elementary  operation  on  Oa ,  Ob,  Oc,  and  Od 
by  multiplying  them  by  elementary  operation  matrices  Ea  = 


03 

— 0.12 


I3 

O 12 


G12 

I3 


I3 

03 

respectively  to  get 


1 

1 

0 

-At/12 

where  012  = 

0 

1 

Aa;i2 

_  0 

0 

1 

I3  - 

012 

o3 

Ec  = 


03 

-Is 


Is 

O12 


03 

Is 


Eb  = 


and  Ed  =  16 


EaOa  =  EbOb  =  EdOd 


EcOc 


I3  0  O13 

0  I3  O23  ’ 

I3  0  O13 

0  I3  O23 

0  0  0 


Therefore,  Lemma  1  implies  that  the  observability  sub-matrix  of  all 
the  four  configurations  have  six  linearly  independent  rows  and  that 
these  rows  span  the  same  observable  space.  ■ 

Remark  5:  The  elementary  operation  matrix  Ed  for  observability 
matrix  Od  in  Lemma  4  is  Identity  because  Od  is  already  in  a  reduced 
row  echelon  form. 

From  Lemma  4  we  can  say  that  the  rows  of  two  edges  for  a  proper 
RPMG  with  a  common  node  are  independent.  The  following  lemma 
is  an  extension  of  this  idea. 

Lemma  5:  If  a  graph  G°  is  a  proper  RPMG  and  has  the  form  of  a 
2-level  tree  (see  figure  3(f))  which  consists  of  a  root  node  and  n  —  1 
leafs  directly  connected  to  the  root,  then  the  rank  of  the  associated 
observability  matrix  is  3 (n  —  1). 


Proof:  Without  loss  of  generality,  assume  that  the  root  node  of 
the  2-level  tree  is  labelled  as  n.  The  system  observability  matrix  will 
then  be  of  the  form 


O2  — level  — 

CO  (-s 

HH  ° 

1 _ 

5*  0 

00 

Oln 

02n 

(22) 

0 

0  ••• 

I3  On  —  l,n 

Clearly,  the  rank  is  3  (n  —  1). 

■ 

Theorem  2:  If  the  graph  G°  is  a  proper  connected  RPMG  then 
the  rank  of  the  associated  observability  matrix  is  3 (n  —  1). 


Proof:  Using  Lemma  4,  any  connected  3-node  subgraph  in  the 
larger  graph  can  be  replaced  with  any  other  connected  3-node  sub¬ 
graph,  without  affecting  the  rank  of  the  system  observability  matrix 
because  their  associated  observability  sub-matrices  span  the  same 
observable  sub-space.  A  connected  graph  G°  can  be  transformed  to 
a  2-level  tree  using  following  algorithm. 

1)  Choose  any  node  and  label  it  as  the  root  as  shown  in  Fig.  3  (a). 

2)  Select  the  nodes  whose  path  from  the  root  consists  of  two 
edges(three  nodes  including  root)  as  shown  in  Fig.  3  (b).  Each 
such  path  can  be  represented  as  a  3-node  subgraph  G3. 

3)  If  a  valid  3-node  subgraph  is  found,  perform  a  subgraph 
replacement  (see  Fig.  3  (c))  so  that  nodes  j  and  i  are  both 
a  distance  of  one  away  from  the  root  node  and  repeat  step  2. 
If  a  valid  subgraph  G3  is  not  found,  continue  to  step  4. 

4)  Search  for  a  three  node  subgraph  G3  that  includes  the  root 
node  and  two  nodes  (nodes  j  and  i)  distance  one  away  from 
the  root  node  that  contain  an  edge  between  these  two  nodes 
(see  Fig.  3  (d)). 

5)  If  a  valid  3-node  subgraph  was  found,  perform  a  subgraph 
replacement  that  maintains  the  edges  between  the  root  node 
and  nodes  i  and  j,  but  removes  the  edge  between  nodes  i  and 
j  (see  Fig.  3  (e)).  Repeat  step  4  until  Go  is  transformed  into 
a  2-level  tree. 

To  show  that  this  algorithm  transforms  a  connected  proper  RPMG  to 
a  2-level  tree,  first  consider  steps  2  and  3.  Every  time  step  2  finds 
a  valid  subgraph,  the  distance  of  node  i  to  the  root  node  will  be 
decreased  from  two  to  one.  Because  the  graph  is  connected,  the  root 
node  will  be  connected  to  any  other  node  within  a  finite  number  of 
steps.  Therefore,  as  steps  2  and  3  continue  to  execute,  all  nodes  will 
be  brought  to  a  maximum  of  distance  one  away  from  the  root  node. 
This  is  similar  to  the  graph  shown  in  Fig.  3(e).  Steps  4  and  5  simply 
remove  any  redundant  edges.  Therefore,  this  algorithm  converts  any 
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A  Robot 
Landmark 
Edge 


connected  graph  to  a  2-level  tree.  The  algorithm  is  also  a  recursive 
way  of  performing  elementary  row  operations  on  the  rows  of  the 
observability  matrices  of  sub-graphs  G°  to  show  that  the  observability 
matrix  of  the  connected  and  proper  RPMG  G°  is  equivalent  to  the 
observability  matrix  of  a  2-level  tree.  Furthermore,  we  can  say  that 
the  basis  of  the  observable  space  of  a  connected  proper  RPMG  G°  is 
the  rows  of  02-uVei  and  from  Lemma  5,  the  rank  of  the  observability 
matrix  is  3 (n  —  1).  ■ 


C.  Observability  Analysis  With  Known  Landmarks 


In  this  subsection,  we  assume  that  landmarks  of  known  location  are 
observed  by  robots  within  the  network,  providing  information  about 
the  global  coordinate  system.  We  derive  conditions  for  complete 
observability  of  the  graph  Gln.  First  we  derive  the  conditions  for 
the  observability  of  a  single  robot. 

Lemma  6:  The  rank  of  the  observability  matrix  of  a  proper  RPMG 
G[  (one  robot  and  l  landmarks)  is  three  if  there  are  at  least  two 
landmarks  (l  >  2)  and  the  robot  and  two  landmarks  are  not  on  the 
same  line  (i.e.,  r/n  ^  rji 2). 


Proof:  Consider  a  proper  RPMG  Gj  with  one  vehicle  and  two 
landmarks  such  that  77,1  rji 2.  Using  (21)  the  observability  matrix 


of  graph  G\  can  be  written  as,  O 


On 

Oi2 


.  To  find  the  number 


of  linearly  independent  rows  we 
on  O  by  multiplying  by  Ei  12  = 


perform  elementary  row  operations 


Ay 


Ayi2 


0 

1 

0 

-1 


Al/j; 

Aj/12 


A«12 

Aa?i2 

Aj/12 


to 


obtain 


E112O  — 


I3 

0lx3 


This  implies  that  two  different  landmarks  provides  three  independent 
rows  to  the  observability  matrix.  Therefore,  from  Theorem  1  the 
single  robot  states  are  completely  observable,  i.e.,  rank(O)  =  3. 


From  Lemma  6  we  know  that,  if  all  of  the  n  vehicles  in  the  group 
are  directly  connected  to  two  different  landmark,  then  the  system  is 
completely  observable  (rank(0)  =  3 n).  However,  due  to  limited 
sensor  range  and  bearing  all  of  the  vehicles  in  the  group  may  not 
be  able  to  see  two  landmarks.  The  following  lemmas  and  theorem 
show  how  cooperative  localization  can  overcome  constraints  posed 
by  sensor  limitations. 

Lemma  7:  Given  a  3-node  RPMG  G2  with  two  robots  and  one 
landmark,  if  the  graph  G2  is  proper  then  the  rows  of  the  observability 
matrix  of  the  two  configurations  of  G2  shown  in  Fig.  4(a)  and 
Fig.  4(b)  spans  the  same  observable  space. 

Proof:  The  two  configurations  shown  in  Fig.  4(a)  and  Fig.  4(b) 
only  differs  in  the  landmark  l  connection.  In  configuration  (a)  the 
landmark  is  directly  connected  to  the  ith  node  whereas  in  configu¬ 
ration  (b)  the  landmark  is  directly  connected  to  the  jth  node.  The 
observability  matrix  for  the  configurations  shown  in  Fig.  4(a)  and  (b) 


Fig.  5.  An  example  of  converting  an  arbitrary  connected  RPMG  with 
landmark  to  a  two-level  tree. 


can  be  written  using  (21)  as 

I3  Oij 
Oil  02x3 


Oa  = 


O  b 


I3 

02x3 


Oij 

Ojl 


(23) 


We  can  perform  elementary  operation  on  Oa  by  multiplying  by 
the  elementary  operation  matrix  ®3x2 


—On 


I2 


to  show 


that  EijiOa  =  Ob-  Therefore,  from  Lemma  1  we  can  say  that  the 
observability  matrix  of  both  the  configurations  spans  the  same  space. 


Lemma  8:  Given  the  RPMG  G„  ,  if  it  is  proper  and  connected 
then  the  associated  observability  matrix  is  equivalent  (observability 
matrix  of  both  graphs  span  the  same  space)  to  the  observability  matrix 
of  a  2-level  tree. 

Proof:  Consider  a  connected  proper  RPMG  G^(example  for  l  = 
1  is  shown  in  Fig.  5(a)).  We  can  write  the  observability  matrix  for 
the  graph  Gln  using  (21).  To  show  that  the  observability  matrix  of 
a  connected  proper  RPMG  Gln  is  equivalent  (observability  matrix  of 
both  graphs  span  the  same  space)  to  the  observability  matrix  of  a 
2-level  tree  we  perform  following  steps. 

•  First  we  perform  elementary  operations  only  on  the  rows  of 
the  observability  matrix  of  the  edges  between  robots  (subgraph 
G°).  From  Theorem  2  we  know  that  these  operations  leads 
to  the  observability  matrix  of  a  2-level  tree.  Therefore,  the 
resulting  observability  matrix  of  the  graph  G„  is  equivalent  to 
the  observability  of  the  3-level  tree  (Example  shown  in  Fig  5(b)) 
with  landmarks  on  level  three.  In  the  3-level  tree  the  path 
between  each  landmark  and  the  root  consists  of  two  edges, 
including  edge  r/j,  between  the  root  and  the  ith  node  (landmark 
is  directly  connected  to  ith  node)  and  edge  r/n  between  landmark 
l  and  the  ith  node.  The  path  between  each  landmark  and  the  root 
can  be  represented  as  a  subgraph  G2  with  two  robots  and  one 
landmark.  There  are  l  such  subgraphs  in  the  graph  Gln. 

•  Next  we  perform  elementary  operations  on  the  rows  of  the 
observability  matrix  of  each  subgraph  G2  associated  with  each 
landmark.  From  Lemma  7  we  know  that  the  resulting  observ¬ 
ability  matrix  of  the  graph  Gln  is  equivalent  to  the  observability 
matrix  of  a  two-level  tree  (see  Fig.  5(c))  with  n  —  1  robot-to- 
robot  leafs  and  l  leafs  between  root  and  landmarks. 

■ 

Theorem  3:  Given  a  proper  RPMG  Gn,  if  for  each  robot  there 
exists  a  path  to  at  least  two  landmarks  and  the  robot  and  two 
landmarks  are  not  on  the  same  line  (i.e.,  ijn  t?i2,  V  i  =  1,  •  •  •  ,  n) 
then  the  system  is  completely  observable,  i.e.,  the  rank  of  the 
observability  matrix  is  3 n. 

Proof:  There  are  two  scenarios  for  paths  between  landmarks.  (1) 
All  the  robots  are  directly  connected  to  the  landmarks.  For  this  case, 
from  Lemma  6  we  know  that  the  proper  RPMG  Gln  is  completely 
observable  if  l  >  2  and  r)u  ^  t)i2,V  t  =  1,  -  ,n.  (2)  Consider  the 

general  case  where  the  proper  RPMG  Gln  is  connected  and  only  a 
subset  of  nodes  measure  landmarks.  In  this  case  all  of  the  robots  are 


7 


not  directly  connected  to  landmarks,  but  there  exists  a  path  between 
each  robot  and  the  landmarks.  From  Lemma  8  we  know  that  the 
observability  matrix  of  a  connected  proper  RPMG  Gln  is  equivalent 
to  the  observability  matrix  of  a  2 -level  tree.  Therefore  connected 
proper  RPMG  Gln  can  be  replaced  by  a  two-level  tree  with  n  —  1 
robot  to  robot  leafs  and  l  leafs  between  the  root  and  landmarks. 
Furthermore,  from  Lemma  5  we  know  that  for  a  2-level  tree  all  of 
the  3(n—  1)  rows  of  the  observability  matrix  of  n—  1  edges  between 
all  robots  are  linearly  independent.  Therefore,  we  only  consider  a 
subgraph  (two-level)  G%  which  consists  of  three  edges,  including  an 
edge  r]ij  between  the  root  (jth  node)  and  ith  node  and  two  edges 
r]ji  and  rjj2  between  the  root  and  two  landmarks.  Using  (21)  the 
observability  matrix  for  this  sub-graph  G \  is  given  by 


O  = 


I3  Oij 
02x3  Ojl 
02x3  Oj2 


(24) 


We  perform  elementary  operation  on  (24)  using 


Eij  12  = 


A Vi2 
At/12 
Aa^i 
Aj/12 

At/12 

AVj2 


At/ii 

At/12 

Axji 

Aj/12 

At/12 
AVj  l 


^2/12 

Axji 

O 

l 

AV12 
Axj  1 

Atji2 

0 

-1 

Atp  2 

AVl2 

AX12 

A2/12 

AV12 

AX12 

Ayi2 

to  obtain  the  reduced  row  echelon  form 


0 

0 

0 

0 

0 

0 

1 


Eij\20  = 


Ie 

0lx6 


(25) 


This  implies  that  two  landmarks  add  three  linearly  independent 
rows  to  the  observability  matrix  of  graph  G2n,  which  are  linearly 
independent  to  the  3(n  —  1)  existing  rows.  Therefore,  the  rank  of  the 
observability  matrix  for  the  RPMG  Gln  with  l  =  2  is  3 n.  ■ 

Additional  videos  of  simulation  and  experimental  results  related  to 
bearing-only  cooperative  localization  can  be  found  in  [22]. 


IV.  Conclusion 

In  this  paper  we  have  shown  that  the  observability  properties  of  a 
system  performing  cooperative  localization  can  be  characterized  by 
the  properties  of  its  relative  position  measurement  graph  (RPMG). 
Using  graph  theoretic  properties  and  nonlinear  observability  theory, 
we  have  shown  that  for  a  connected  proper  RPMG  G ^  without 
landmarks,  the  maximum  rank  of  the  observability  matrix  is  3(n—  1). 
Furthermore,  we  have  shown  that  to  achieve  full  observability,  all 
nodes  in  the  graph  must  have  a  path  to  at  least  two  different  landmarks 
of  known  location. 
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